package neves.android.etmg.pathfinding;

import java.util.LinkedList;
import java.util.List;

import android.util.Log;

public class AStarSearch {
	
	public static class PriorityList extends LinkedList{
		public void add(Comparable object){
			for(int i=0;i<size();i++){
				if(object.compareTo(get(i))<=0){
					add(i,object);
					return;
				}
			}
			addLast(object);
		}
	}
	
	protected List constructPath(AStarNode node){
		LinkedList path = new LinkedList();
		while(node.pathParent!=null){
			//Log.w("ETMG",node.getNodeInfo()+"P:"+node.pathParent.getNodeInfo());
			path.addFirst(node);
			node=node.pathParent;
		}
		return path;
	}
	public List findPath(AStarNode startNode,AStarNode goalNode){
		PriorityList openList = new PriorityList();
		LinkedList closedList = new LinkedList();
		
		startNode.costFromStart = 0;
		startNode.estimatedCostToGoal=startNode.getEstimatedCost(goalNode);
		startNode.pathParent = null;
		openList.add(startNode);
				
		while(!openList.isEmpty()){
			
			AStarNode node = (AStarNode)openList.removeFirst();
			
			
			//priintPath(constructPath(goalNode)); //for debug 
			
			if(node==goalNode){
				return constructPath(goalNode);
			}
			
			List neighbors = node.getNeigbors();
			for(int i=0;i<neighbors.size();i++){
				AStarNode neighborNode = (AStarNode)neighbors.get(i);
				boolean isOpen = openList.contains(neighborNode);
				boolean isClosed = closedList.contains(neighborNode);
				int costFromStart = node.costFromStart + node.getCost(neighborNode);
								
				if((!isOpen && !isClosed) || costFromStart < neighborNode.costFromStart){
					neighborNode.pathParent = node;
					neighborNode.costFromStart = costFromStart;
					neighborNode.estimatedCostToGoal = neighborNode.getEstimatedCost(goalNode);
					if(isClosed){
						closedList.remove(neighborNode);
					}
					if(!isOpen){
						openList.add(neighborNode);
					}
				}
				
			}
			closedList.add(node);
		}
		return null;
	}
	
	private void priintPath(List path){
		
		String pathString = "";
		
		for(int temi =0 ; temi<path.size();temi++){
			
			pathString += ((AStarNode)path.get(temi)).getNodeInfo()+";";
			
		}
		Log.w("ETMG",pathString);
	}
	
	
}
